/*
 * Copyright (c) 2021, Texas Instruments Incorporated
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * *  Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 *
 * *  Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *
 * *  Neither the name of Texas Instruments Incorporated nor the names of
 *    its contributors may be used to endorse or promote products derived
 *    from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
 * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
 * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
 * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
 * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
 * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
 * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
 * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
 * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
 * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
//fuck
/**
 * @file empty.c
 * @brief 主程序文件，实现四个编码电机以PID速度控制方式运行
 * 
 * 本程序初始化系统、编码器和电机控制，并在主循环中
 * 不断读取编码器状态，使用PID算法控制电机以设定的速度运行。
 * 
 * 使用的硬件资源:
 * - 四个PWM通道输出(PA0,PA1,PA3,PA12)
 * - 四个正交编码器输入(PA5/PA4, PA8/PA27, PA26/PA25, PA10/PA11)
 * - 八个电机方向控制引脚(GPIOB的多个引脚)
 */

#include "ti_msp_dl_config.h"
#include "encoder.h"
#include "pid.h"
#include <stdbool.h>
#include "OLED.h"

// 定义电机控制常量
#define MOTOR1_SPEED 10  // 设置电机速度为50%
#define MOTOR2_SPEED 10  // 设置电机速度为50%
#define MOTOR3_SPEED 10  // 设置电机速度为50%
#define MOTOR4_SPEED 10  // 设置电机速度为50%
#define PID_UPDATE_INTERVAL 10 // PID更新间隔(毫秒)

// 定义电机速度目标值(单位: 编码器计数/秒)
#define MOTOR1_TARGET_SPEED 1000
#define MOTOR2_TARGET_SPEED 1000
#define MOTOR3_TARGET_SPEED 1000
#define MOTOR4_TARGET_SPEED 1000

// 定义PID控制器
PID_t motor1_pid;
PID_t motor2_pid;
PID_t motor3_pid;
PID_t motor4_pid;

// 记录上次编码器计数，用于计算速度
int32_t prev_encoder1_count = 0;
int32_t prev_encoder2_count = 0;
int32_t prev_encoder3_count = 0;
int32_t prev_encoder4_count = 0;

// 记录上次PID更新时间
uint32_t last_pid_update_time = 0;

/**
 * @brief 简单延时函数
 * 
 * 提供一个基于循环的延时功能，用于简单的时序控制
 * 注意：这不是精确的时间延迟，依赖于CPU时钟频率
 * 
 * @param count 延时的时间单位秒
 */
void delay(uint32_t count)
{   
    uint32_t temp;
    temp = count * 32000000
    delay_cycles(temp);
}

// /**
//  * @brief 获取系统时间(毫秒)
//  * 
//  * 注意：这是一个简化实现，实际应用中应使用硬件定时器
//  * 
//  * @return uint32_t 系统时间(毫秒)
//  */
// uint32_t get_time_ms(void)
// {
//     // 简化实现，实际应使用硬件定时器
//     static uint32_t time_ms = 0;
//     time_ms += 1; // 假设每次调用增加1ms
//     return time_ms;
// }

/**
 * @brief 初始化PID控制器
 */
void PID_Control_Init(void)
{
    // 初始化电机1的PID控制器
    // 参数(Kp, Ki, Kd)需要根据实际电机特性调整
    PID_Init(&motor1_pid, 0.5f, 0.1f, 0.01f, 0.0f, 100.0f);
    PID_SetTarget(&motor1_pid, MOTOR1_TARGET_SPEED);
    
    // 初始化电机2的PID控制器
    PID_Init(&motor2_pid, 0.5f, 0.1f, 0.01f, 0.0f, 100.0f);
    PID_SetTarget(&motor2_pid, MOTOR2_TARGET_SPEED);
    
    // 初始化电机3的PID控制器
    PID_Init(&motor3_pid, 0.5f, 0.1f, 0.01f, 0.0f, 100.0f);
    PID_SetTarget(&motor3_pid, MOTOR3_TARGET_SPEED);
    
    // 初始化电机4的PID控制器
    PID_Init(&motor4_pid, 0.5f, 0.1f, 0.01f, 0.0f, 100.0f);
    PID_SetTarget(&motor4_pid, MOTOR4_TARGET_SPEED);
}

/**
 * @brief 更新电机速度控制
 * 
 * 根据编码器反馈，使用PID算法更新电机速度
 */
void update_motor_speed_control(void)
{
    // 读取各个编码器的当前计数
    int32_t encoder1_count = Encoder1_GetCount();
    int32_t encoder2_count = Encoder2_GetCount();
    int32_t encoder3_count = Encoder3_GetCount();
    int32_t encoder4_count = Encoder4_GetCount();
        
    // 计算速度(编码器计数变化/时间间隔)
    // 转换为每秒的计数变化
    float motor1_speed = (float)(encoder1_count - prev_encoder1_count) * (1000.0f / PID_UPDATE_INTERVAL);
    float motor2_speed = (float)(encoder2_count - prev_encoder2_count) * (1000.0f / PID_UPDATE_INTERVAL);
    float motor3_speed = (float)(encoder3_count - prev_encoder3_count) * (1000.0f / PID_UPDATE_INTERVAL);
    float motor4_speed = (float)(encoder4_count - prev_encoder4_count) * (1000.0f / PID_UPDATE_INTERVAL);
        
        // 保存当前编码器计数，用于下次计算
        prev_encoder1_count = encoder1_count;
        prev_encoder2_count = encoder2_count;
        prev_encoder3_count = encoder3_count;
        prev_encoder4_count = encoder4_count;
        
        // 使用PID计算新的控制输出
        float motor1_output = PID_Compute(&motor1_pid, motor1_speed);
        float motor2_output = PID_Compute(&motor2_pid, motor2_speed);
        float motor3_output = PID_Compute(&motor3_pid, motor3_speed);
        float motor4_output = PID_Compute(&motor4_pid, motor4_speed);

        OLED_ShowNum(1, 1, (uint32_t)motor1_output, 3);
        OLED_ShowNum(1, 4, (uint32_t)motor2_output, 3);
        OLED_ShowNum(1, 7, (uint32_t)motor3_output, 3);
        OLED_ShowNum(1, 10, (uint32_t)motor4_output, 3);

        // 应用PID输出到电机
        // 注意：这里假设Motor_SetSpeed接受0-100的值作为速度百分比
        Motor_SetSpeed(1, (uint32_t)motor1_output);
        Motor_SetSpeed(2, (uint32_t)motor2_output);
        Motor_SetSpeed(3, (uint32_t)motor3_output);
        Motor_SetSpeed(4, (uint32_t)motor4_output);    
}

/**
 * @brief 主函数
 * 
 * 初始化系统并启动主循环
 * 
 * @return int 程序执行状态，0表示正常退出(此程序不会退出)
 */
int main(void)
{
    uint8_t i;
    // 初始化系统配置 - 配置时钟、电源和外设
    SYSCFG_DL_init();
    OLED_Init();
    // 初始化编码器 - 准备读取编码器状态
    Encoder_Init();
    // 初始化PID控制器
    PID_Control_Init();
    //使能定时中断
    NVIC_EnableIRQ(TIMER_IR_INST_INT_IRQN);
    DL_TimerA_startCounter(TIMER_IR_INST);
    //使能GPIO中断
    NVIC_EnableIRQ(Sensor_GPIOA_INT_IRQN);
    
    
    // 设置初始电机速度
    Motor_SetSpeed(1, MOTOR1_SPEED);
    Motor_SetSpeed(2, MOTOR2_SPEED);
    Motor_SetSpeed(3, MOTOR3_SPEED);
    Motor_SetSpeed(4, MOTOR4_SPEED);

    
    // 主循环 - 程序将一直在此循环中运行
    while (1) {



        
        // // 这里可以添加其他处理逻辑
        // // 例如: 
        // // 1. 系统状态监测
        // // 2. 通信处理
        // // 3. 用户界面更新
        
        // // 短暂延时，让系统有时间响应
        // delay(100);
    }
}

/**
 * @brief 定时器A0中断处理函数
 * 
 * 处理PWM定时器的中断事件
 * 该函数使用弱符号属性，如果用户提供了另一个实现，链接器将使用用户的版本
 */
void TIMER_IR_INST_IRQHandler(void) 
{
    // 更新编码器状态 - 读取编码器信号变化并更新计数
    Encoder_Update();
        
    // 更新电机速度控制 - 使用PID算法调整电机速度
    update_motor_speed_control();


    // 清除中断标志位，防止重复触发
    DL_TimerA_clearInterruptStatus(PWM_morton_INST, DL_TIMER_INTERRUPT_ZERO_EVENT);
    
    
    // 在中断中可以添加其他处理逻辑
    // 例如:
    // 1. PWM参数动态调整
    // 2. 精确定时的任务执行
    // 3. 电机保护功能
}

